/* Includes the Standard IO Library */
#include <stdio.h>
#include <stdlib.h>
/*
 *  roverSide.h
 *  CommandCenter08
 *
 *  Created by David P. Miller on 7/22/08.
 *  Copyright 2008 University of Oklahoma. All rights reserved.
 *
 */

#include "roverSide.h"

// Convert from compass angles where N=0 and E=90 to Cartesian where N=90 and E=0
float compassToCartDegrees(float a)
{
	a = 360 - (a-90);
	if(a > 360) a -= 360;
	return a;
}

// Get an angle between 0 and 360
float normalizeDegAngle(float a)
{
	if(a >=360.0) return(normalizeDegAngle(a-360.0));
	else {
		if(a < 0.0)return(normalizeDegAngle(a+360.0));
		else return (a);
	}
}

// Get an angle between 0 and TWOPI
float normalizeRadAngle(float a)
{
	if(a >=TWOPI) return(normalizeRadAngle(a-TWOPI));
	else {
		if(a <0.0)return(normalizeRadAngle(a+TWOPI));
		else return (a);
	}
}

//Finds the average angle in degrees of two angles
//Presumes the two headings make an acute angle
//Converts angle from compass degrees (CW, N=0) to
//XY angle (CCW, E=0)
float avgHeadingToDegrees(float deg1, float deg2)
{
	float r1=normalizeDegAngle(deg1);
	float r2=normalizeDegAngle(deg2);
	float avg;
	avg=(r1+r2)/2.0;
	if((MAXIMUM(r1,r2)-MINIMUM(r1,r2))>180)//two heading make obtuse angle...
		avg=normalizeDegAngle(avg+180);//...go for the actute average
	//avg=normalizeRadAngle(HALFPI - avg);//change to XY rotation
	avg=compassToCartDegrees(avg);
	return(avg);
}

//find the distance in the XY plane of two points
float distanceBetweenPoints(Point3D p1, Point3D p2)
{
	float xdiff=p1.x - p2.x;
	float ydiff=p1.y - p2.y;
	float dist;
	dist=sqrt(xdiff*xdiff + ydiff*ydiff);
	return (dist);
}

// takes pointer to rover position and the current values for encoders and compass
// and updates the rover structure with those new values and the new position as
// calculated from those values and the rover's previous position.
void updateRoverPosition (roverProperties *rover, int cltics, int crtics, float ccompass)
{
	float avghead=avgHeadingToDegrees(rover->compass, ccompass);
	float ldiff=cltics - rover->leftTicks;
	float rdiff=crtics - rover->rightTicks;
	float dist,xInc, yInc, dt;
	// these are used for calculating stalls
	static int prevLeftSpeed=0, prevRightSpeed=0, prevLeftStall=0, prevRightStall=0, prevTurnStall=0, pTime=0;
	
	dist = PI*rk.WHEELDIAM*(ldiff/rk.LTICSPERREV+rdiff/rk.RTICSPERREV)/2.0;
	if(dist > rk.TELEPORTDISTANCE){
		callForHelp(rover, REPosition, dist);
	}
	else{
		rover->odometer+=dist;
		yInc = dist*sin(DEGTORAD(avghead));
		xInc = dist*cos(DEGTORAD(avghead));
		rover->leftTicks=cltics;
		rover->rightTicks=crtics;
		
		rover->turnStall=0;
		if(((rover->rightSpeed - rover->leftSpeed) > rk.TURNDIFF) && ((prevRightSpeed - prevLeftSpeed) > rk.TURNDIFF)){ //there is a tight left turn in progress
			if((ccompass < rover->compass) || ((ccompass-180.0) > rover->compass)){ // there is NOT a turnstall
				rover->turnStall = prevTurnStall = 0;
			}
			else if(prevTurnStall) rover->turnStall=1; //stalled two updates in a row?
			else prevTurnStall=1;
		}
		else {
			if(((rover->leftSpeed - rover->rightSpeed) > rk.TURNDIFF) && ((prevLeftSpeed - prevRightSpeed) > rk.TURNDIFF)){ //tight right turn in progress
				if((ccompass > rover->compass) || ((ccompass+180.0) < rover->compass)){ // there is NOT a turnstall
					rover->turnStall = prevTurnStall = 0;
				}
				else if(prevTurnStall)rover->turnStall=1; //stalled two updates in a row?
				else prevTurnStall=1;
			}
			else rover->turnStall = prevTurnStall = 0; // no turn in progress, clear stall indicators
		}		
		rover->compass = ccompass;
		rover->position.x = rover->position.x + xInc;
		rover->position.y = rover->position.y + yInc;
		dt=(float)(rover->xbctime - pTime)/1000.0;
		
		if(rover->leftSpeed && prevLeftSpeed && rover->leftSpeed==prevLeftSpeed){// has set speed been constant?
			if(ldiff < (int)((float)prevLeftSpeed*dt*rk.STALLPERCENTAGE)){//did rover move less than a fraction of expected amount?
				if(prevLeftStall) rover->leftStall=1;//has this condition persisted two or more updates?
				prevLeftStall=1; // if this is the first (in a row) mark it
			}
			else{
				prevLeftStall=0; // stall condition does not exist
				rover->leftStall=0;
			}
		}
		else{
			prevLeftStall=0;// prerequisites for stall condition did not exist
			rover->leftStall=0;
		}//end left stall tests
		
		if(rover->rightSpeed && prevRightSpeed && rover->rightSpeed == prevRightSpeed){
			if(rdiff < (int)((float)prevRightSpeed*dt*rk.STALLPERCENTAGE)){
				if(prevRightStall) rover->rightStall=1;
				prevRightStall=1;
			}
			else{
				prevRightStall=0;
				rover->leftStall=0;
			}
		}
		else{
			prevRightStall=0;
			rover->rightStall=0;
		}//end right stall tests
		
		prevLeftSpeed = rover->leftSpeed;
		prevRightSpeed = rover->rightSpeed;
		pTime = rover->xbctime;	
	}//end valid rover update
}

// stops rover and says error message; data is one value of data (e.g., overlarge update dist
void callForHelp(roverProperties *rover, int errCode, int data) {
	rover->leftSpeed = rover->rightSpeed = 0;
	rover->state=RSCallingForHelp;
	rover->error = errCode;
}


// Calculates the compass heading in degrees to the waypoint from the rover's current position
float headHereToThere(roverProperties *rover)
{
	float xDiff=rover->wp->position.x - rover->position.x;
	float yDiff=rover->wp->position.y - rover->position.y;
	float a;
	if(xDiff == 0.0 && yDiff >= 0.0) return (0.0);
	if(xDiff == 0.0 && yDiff < 0.0) return (180.0);
	if(yDiff == 0.0 && xDiff >= 0.0) return (90.0);
	if(yDiff == 0.0 && xDiff < 0.0) return (270.0);
	a=atan2(yDiff,xDiff); // compute the angle
	a=compassToCartDegrees(RADTODEG(a));// change to compass rotation
	return(a);
}

// returns a turning angle in degrees to head towards the current waypoint
float turnAngleTowardsThere(roverProperties *rover)
{
	float absHead = headHereToThere(rover);
	float relHead;
	relHead = absHead - rover->compass;
	if(relHead < -180) return(relHead+360);
	if(relHead > 180) return(relHead-360);
	return(relHead);
}

// returns the distance between rover and way point
float distanceHereToThere(roverProperties *rover)
{
	return distanceBetweenPoints(rover->wp->position, rover->position);
}

// gets the rover turning the requested amount (large turns are done as point turns. Small turns done gradually).
// this function just sets the speeds and returns. relHead is in compass format degrees
void turnToHeading(roverProperties *rover, float relHead)
{
	//static int pLeftStall=0, pRightStall=0;
	if(relHead > 0.0){
		rover->leftSpeed = rk.POINTTURNSPEED;
		if(relHead > rk.POINTTURNANGLE)// sharp right turn
			rover->rightSpeed = -rk.POINTTURNSPEED;
		else rover->rightSpeed = rk.POINTTURNSPEED - relHead * rk.TURNMULT;
	}
	else{
		rover->rightSpeed = rk.POINTTURNSPEED;
		if(relHead < -rk.POINTTURNANGLE)// sharp left turn
			rover->leftSpeed = -rk.POINTTURNSPEED;
		else  rover->leftSpeed = rk.POINTTURNSPEED + relHead * rk.TURNMULT;
	}
	
	/*
	 if(rover->rightStall && rover->leftStall && rover->turnStall) callForHelp(rover, REStall, 0);
	 else if(rover->rightStall || pRightStall){
	 if(!pRightStall) pRightStall = rover->xbctime;
	 if((rover->xbctime - pRightStall) > rk.PTSTALLTIME) pRightStall = 0;
	 else rover->rightSpeed = rover->rightSpeed/(-3);
	 }
	 else if(rover->leftStall || pLeftStall){
	 if(!pLeftStall) pLeftStall = rover->xbctime;
	 if((rover->xbctime - pLeftStall) > rk.PTSTALLTIME) pLeftStall = 0;
	 else rover->leftSpeed = rover->leftSpeed/(-3);
	 }*/
}

//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//***********************************************************************************
// HIGHEST LEVEL FUNCTION CALL
//Moves robot towards waypoint.  When WP is reached it stops rover and returns -1, otherwise
// the distance to the waypoint is returned. Note if WP is almost close enough and rover is avoiding obstacles, that WP will be skipped
float moveToWaypoint(roverProperties *rover)
{
	static int progressWPID = -1;
	static float progressGoneToFar;
	static int avoiding=0;
	static int turnNotCruise = 0, lastObstacleTurnDir = 0;
	static Point3D lastObstaclePosition = {1000000.0, 1000000.0, 0.0};
	float wpDistance = distanceHereToThere(rover), obstacleDirection;
	float relHead = turnAngleTowardsThere(rover);
	
	//printf("relative heading %f heading %f\n",relHead,rover->compass);
	
	if(wpDistance < rk.CLOSEENOUGH){//Is rover near a WP -- don't worry just mark it done
		//rover is near its waypoint -- congratulations
		rover->leftSpeed = rover->rightSpeed = 0;
		return (-1.0);
	}
	if(progressWPID == rover->wp->uuid){
		if(progressGoneToFar < rover->odometer){ //Rover Needs Help!
			callForHelp(rover, REProgress, progressWPID); //setting help
			return(-3.0); // rover not making progress
		}
	}//rover still seems to be progressing
	else{ // progressWPID is out of date, so update it
		progressWPID = rover->wp->uuid;
		progressGoneToFar = rover->odometer + rk.PATHEFFICIENCY*distanceHereToThere(rover);
	}//rover progressing and updating progress metrics
	
	
	// in all other circumstances...
	obstacleDirection = checkForDistantOsbtacles(rover, wpDistance); // check for obstacles
	
	if(rover->state == RSCallingForHelp) return(-rover->error); //obstacle check called an emergency
	
	if((rover->state == RSAvoidingDistantObstacles) || (rover->state == RSAvoidingNearbyObstacles)){// if an obstacle was seen...
		lastObstaclePosition = rover->position; //... mark this spot as latest position where obstacle was seen
		if(lastObstacleTurnDir == 0){//are we already in an avoidance turn -- NO
			if(obstacleDirection < 0.0) lastObstacleTurnDir = -1;
			else lastObstacleTurnDir = 1;
		}//end not already in a turn
		/*		else{//change obstacleDirection to match exitsting turn
		 obstacleDirection = (lastObstacleTurnDir < 0)? -1.0 : 1.0;
		 }
		 */	}// end obstacle currently in view
	else{//Are we sufficiently far from last spot where we saw obstacle?
		if(distanceBetweenPoints(rover->position, lastObstaclePosition) > rk.GOPASTDIST){//Far enough, so reset stuff
			lastObstacleTurnDir = 0;
			rover->state = RSMovingTowardsWaypoint;
		}//nothing in view, stop avoiding
		else{//don't resume going after WP quite yet
			rover->state = RSGoingPastObstacles; // this is the state for going past obstacles
		}//end continue avoiding though nothing in view
	}//end far enough away check
	
	avoiding = (rover->state == RSAvoidingDistantObstacles) || (rover->state == RSAvoidingNearbyObstacles) || (rover->state == RSGoingPastObstacles);
	if(wpDistance < 2.0*rk.CLOSEENOUGH && avoiding){
		//rover is almost close enough, but there are obstacles keeping it from getting to the waypoint
		rover->leftSpeed = rover->rightSpeed = 0;
		//printf("should not be here\n");
		return (-2.0);
	}
	else{
		if(!avoiding){
			//&& ((fabs(relHead) > rk.NEEDTURNCORRECTION) || turnNotCruise)){ // turning
			//printf("point turn: rNC:%d rH:%.2f rNTC%.2f \n", turnNotCruise, relHead, rk.NEEDTURNCORRECTION);
			if(fabs(relHead) > rk.TURNACCURACYLIMIT) {
				turnNotCruise=1;
				//printf("TToH TAL:%.2f\n", rk.TURNACCURACYLIMIT);
				turnToHeading(rover, relHead);
			}
			else {
				turnNotCruise = 0;
				rover->leftSpeed = rover->rightSpeed = rk.ROVERCRUISESPEED;
				//printf("Cruising-0...\n");
			}
			return(wpDistance);
		}
		//Avoiding obstacle or going straight
		else{
			avoidingTurn(rover, obstacleDirection, lastObstacleTurnDir);
			//if(lastObstacleTurnDir == 0 || obstacleDirection == 0.0) avoiding = 0;
		}
		return(wpDistance);
	}
}

//This function keeps the rover going straigh past avoided obstacles, turns wide or sharp or extreme as needed
void avoidingTurn(roverProperties *rover, float obstacleDirection, int lastObstacleTurnDir)
{
	float closeTurnFactor = (rk.TURNFACTOR > 0.0)?0.0:rk.TURNFACTOR;
	
	if(rover->state == RSGoingPastObstacles){	//no obstacle in view, but going past previously seen obstacle, so no turn
		rover->leftSpeed = rover->rightSpeed = rk.ROVERCRUISESPEED;
		//printf("Cruising-1...\n");
		return;
	}
	if(lastObstacleTurnDir){
		if(lastObstacleTurnDir < 0){//already in a right turn
			if(obstacleDirection < 0.0 || (obstacleDirection > 0.0 && rover->state == RSAvoidingDistantObstacles)){//obstacle to the left or far off to the right
				//printf("right turn\n");
				rover->leftSpeed=rk.ROVERCRUISESPEED;
				rover->rightSpeed=rk.ROVERCRUISESPEED*((rover->state == RSAvoidingNearbyObstacles)?closeTurnFactor:rk.TURNFACTOR);
				return;
			}
			else if(obstacleDirection > 0.0){//obstacle close to the right
				//printf("reverse right turn\n");
				rover->leftSpeed=0;
				rover->rightSpeed=-rk.ROVERCRUISESPEED;
				return;
			}
		}
		else{//already in left turn
			if(obstacleDirection > 0.0 || (obstacleDirection < 0.0 && rover->state == RSAvoidingDistantObstacles)){//obstacle to the right or far off to the left
				//printf("right turn\n");
				rover->rightSpeed=rk.ROVERCRUISESPEED;
				rover->leftSpeed=rk.ROVERCRUISESPEED*((rover->state == RSAvoidingNearbyObstacles)?closeTurnFactor:rk.TURNFACTOR);
				return;
			}
			else if(obstacleDirection < 0.0){//obstacle close to the right
				//printf("reverse right turn\n");
				rover->rightSpeed=0;
				rover->leftSpeed=-rk.ROVERCRUISESPEED;
				return;
			}
		}
	}
}


// there is an obstacle nearby, so stop, backup if needed, turn turnAvoidDirection to avoid the obstacle
// then drive past the obstacle.  When that is completed turn rover state back to normal
void avoidNearbyObstacle(roverProperties *rover) {}

// Is there an obstacle about to be hit?
void checkForImmediateObstacles(roverProperties *rover){}

// Is there an obstacle closer than the WP?
float checkForDistantOsbtacles(roverProperties *rover, float wpDistance)
{
	int aheadBlockedLong=0, aheadBlockedShort=0, instShort=0,instLong=0, i;
	int critDistance = ((int)(wpDistance*1000.0));
	float heights[rk.PANELSEGMENTS];
	float aheadBlockedDir=0,tmp;
	static int prevBlockedShort=0, prevBlockedLong=0;
	
	//	if(rover->state==RSAvoidingDistantObstacles) return; //already avoiding
	getPanelHeights(rover, heights);
	//	for(i=0;i<16;i++)printf("h%d %.2f;",i,heights[i]);
	critDistance = (critDistance < rk.BODYDIST)?critDistance:rk.BODYDIST; //set to minimum of critDistance and BODYDIST
	
	tmp=profileNoPosObstacle(rover); 
	//printf("Profile slope %.2f tilt:%.2f sum:%.2f\n",tmp,rover->tilt,tmp+rover->tilt);
	
	aheadBlockedShort = aheadBlockedLong = aheadBlockedDir = 0;
	if(rk.BODYOBSTACLES && (!rk.SLOPEOBSTACLES || (rk.SLOPEOBSTACLES && rover->tilt > rk.PITCHDOWNIGNOREDISTOBSTACLES)))
		for(i=4;i<12;i++){
			if(rover->bodyScanner->rangeData[i]< critDistance && rover->bodyScanner->rangeData[i+1]< critDistance){//is the way ahead clear?
				if(prevBlockedLong)aheadBlockedLong=1;
				else prevBlockedLong=instLong=1;
				aheadBlockedDir = aheadBlockedDir + 1/(i - 7.5);//+ is blocked on right, - is blocked on left
			}
		}
	if(rk.PANELOBSTACLES){
		for(i=1;i<5; i++){//right wheel obstacles
			if((fabs(heights[16-i]) > rk.PROFILEOBSTACLEMAX) && (fabs(heights[16-i+1]) > rk.PROFILEOBSTACLEMAX) && (fabs(heights[16-i+2]) > rk.PROFILEOBSTACLEMAX)){//is there a rock or hole?
				if(prevBlockedShort)aheadBlockedShort=1;
				else prevBlockedShort=instShort=1;
				aheadBlockedDir = aheadBlockedDir + 1/(i - 7.5);//+ is blocked on right, - is blocked on left
				//printf("Panel Obstacle %d IS %d PS %d BS:%d\n",i, instShort, prevBlockedShort,aheadBlockedShort);
			}
		}
		for(i=11;i<15; i++){//left wheel obstacles
			if((fabs(heights[16-i]) > rk.PROFILEOBSTACLEMAX) && (fabs(heights[16-i-1]) > rk.PROFILEOBSTACLEMAX)&& (fabs(heights[16-1-2]) > rk.PROFILEOBSTACLEMAX)){//is there a rock or hole?
				if(prevBlockedShort)aheadBlockedShort=1;
				else prevBlockedShort=instShort=1;
				aheadBlockedDir = aheadBlockedDir + 2/(i - 7.5);//+ is blocked on right, - is blocked on left
				//printf("Panel Obstacle %d IS %d PS %d BS:%d\n",i, instShort, prevBlockedShort,aheadBlockedShort);
			}
		}
	}
	if(rk.SLOPEOBSTACLES){
		if(rover->tilt > rk.MAXPITCH){// Pitch is too high
			if(fabs(rover->roll) < rk.MAXROLL){ // roll is within limits
				aheadBlockedLong=1;
				if(rover->roll > 0.0) aheadBlockedDir=5; //turn right
				else  aheadBlockedDir=-5; //turn left
			}
			else{// roll is also maxed out
				callForHelp(rover, REPitch, 4); //setting help
				return(-4.0); // Roll is maxed out!!!!!!
			}
		}
		else if(fabs(rover->roll)> rk.MAXROLL){// roll maxed out by itself is an emergency
			callForHelp(rover, RERoll, 4); //setting help
			return(-4.0); // Roll is maxed out!!!!!!
		}
	}
	if(rk.BODYOBSTACLES)
		for(i=0;i<15;i++){//straight is blocked far, how are things close up
			if(rover->bodyScanner->rangeData[i]< rk.BODYTOOCLOSE*1000){
				if(prevBlockedShort)aheadBlockedShort=1;
				else prevBlockedShort=instShort=1;
				aheadBlockedDir = aheadBlockedDir + 2/(i - 7.5);//+ is blocked on right, - is blocked on left
			}
		}
	if(!instShort && !aheadBlockedShort)prevBlockedShort=0; //no hits this time, reset two in a row counter
	if(!instLong && !aheadBlockedLong)prevBlockedLong=0;
	if(aheadBlockedDir == 0.0 && (aheadBlockedShort || aheadBlockedLong))aheadBlockedDir=-2.; //something directly ahead turn right
	if(aheadBlockedLong)rover->state = RSAvoidingDistantObstacles; //mark that you are avoiding
	if(aheadBlockedShort)rover->state = RSAvoidingNearbyObstacles;
	if (!aheadBlockedLong && !aheadBlockedShort) rover->state = RSMovingTowardsWaypoint;
	return(aheadBlockedDir);
}
/*
 
 
 
 if(terrainProfile(rover, &obstDist, deviation, &proIdx) < 0 &&
 obstDist < critDistance && 
 proIdx > 0 && 
 fabs(deviation[proIdx]-deviation[proIdx-1]) > fabs(rk.PROFILEOBSTACLEMIN)){
 //rover->state=RSAvoidingDistantObstacles;
 //return;
 }
 
 for(i=5;i<11;i++){
 if(rover->bodyScanner->rangeData[i] < critDistance){
 if(rover->bodyScanner->rangeData[i] > rk.BODYTOOCLOSE &&
 //profileNoPosObstacle(deviation) &&
 rover->tilt < rk.MAXPITCH/2.0 && 
 ((i<8) && rover->roll < rk.MAXROLL/2.0) || ((i>=8) && rover->roll > -rk.MAXPITCH/2.0));
 else{
 rover->state=RSAvoidingDistantObstacles;
 return;
 }
 }
 }
 
 }
 }
 */

// Get the distance and a heading that avoids obstacle from the body laser. BODYDIST is the horizon distance for the body laser
void obstacleDistanceAndClearHeading(roverProperties *rover, float *obstacleDistance, float *clearHeading)
{
	int minDist=5000, i,j=0,clear[16];
	float wedgeDirection=0;
	float tempDirection;
	int clearWedge[4][2]={{-1,0},{-1,0},{-1,0},{-1,0}}; // contains the ray number for the start of a wedge and the width of the wedge
	
	for(i=4;i<12;i++) if(rover->bodyScanner->rangeData[i] < minDist) minDist=rover->bodyScanner->rangeData[i];
	*obstacleDistance=minDist;
	*clearHeading = rover->compass;
	if(minDist >= rk.BODYDIST) return;
	
	//Note that scanner data is organized CCW, so it is converted to CW below to match compass
	for(i=15;i>=0;i--){
		if(rover->bodyScanner->rangeData[15-i] >= rk.BODYDIST) clear[i]=1; 
		else clear[i]=0;
	}
	
	// finds wedges in groups of three or more adjacent rays maximum of 4 wedges possible
	for(i=0;i<13;i++){
		if(clear[i] && clear[i+1] && clear[i+2]){
			if(clearWedge[j][0]<0){
				clearWedge[j][0]=i;
				clearWedge[j][1]=2;
			}
			else clearWedge[j][1]++;
		}
		else if(clearWedge[j][0]>=0) j++;
	}
	
	// find the wedge closest to the forward direction
	for(i=0;i<=j;i++){
		if(clearWedge[i][0]+clearWedge[i][1]/2 < 8){
			tempDirection = clearWedge[i][0]+clearWedge[i][1]-1.5;
		}
		else tempDirection = clearWedge[i][0]+1.5;
		
		if(fabs(7-tempDirection) < fabs(7-wedgeDirection)) wedgeDirection = tempDirection;
		//printf("Wedge %d start %d width %d\n",i,clearWedge[i][0],clearWedge[i][1]);
	}
	printf("closest wedge %f\n",wedgeDirection);
	if(j!=0){
		*clearHeading = rover->compass+(7.0-wedgeDirection)*RADTODEG(rover->bodyScanner->dAlpha);
		*clearHeading = normalizeDegAngle(*clearHeading);
		printf("%f clear head %f wedges %f\n",rover->compass,*clearHeading,(7.0-wedgeDirection)*RADTODEG(rover->bodyScanner->dAlpha));
	}
}


// there is not an obstacle within 1m and there is an obstacle closer than BODYDIST away and the upcoming waypoint is past
// that distance, so turn the rover turnAvoidDirection to go to the side of the upcoming obstacle. When
// the rover is past the upcoming obstacle, switch the rover state back to normal
void avoidDistantObstacle(roverProperties *rover) 
{
	static float obstacleDistance=-1.0, clearHeading; //clearHeading is the compass heading closest to the waypoint heading that looks clear
	static Point3D oriPos;
	float cObstacleDistance, cClearHeading, cDist;
	
	if(obstacleDistance<=0.0){ //this is the first time this function has been called for this obstacle
		oriPos.x = rover->position.x;
		oriPos.y = rover->position.y;
		obstacleDistanceAndClearHeading(rover, &obstacleDistance, &clearHeading);
		if((clearHeading - rover->compass) > 0.0 && (clearHeading - rover->compass) < 180.0) rover->turnAvoidDirection=1;
		else rover->turnAvoidDirection=-1;
	}
	else{ // rover has traveled past the obstacle so reset everything
		if((cDist=distanceBetweenPoints(oriPos,rover->position)) > obstacleDistance){
			obstacleDistance = -1.0;
			rover->turnAvoidDirection = 0;
			rover->state = RSMovingTowardsWaypoint;
		}
		else{//avoid the obstacle
			obstacleDistanceAndClearHeading(rover, &cObstacleDistance, &cClearHeading);
			if((obstacleDistance - cDist) > cObstacleDistance){//Is there a closer obstacle? Is it because we have not turned enough yet?
				if((((clearHeading - rover->compass) > 0.0 && (clearHeading - rover->compass) < 180.0)?1:-1)==rover->turnAvoidDirection){
					turnToHeading(rover, (clearHeading - rover->compass + rk.TURNACCURACYLIMIT*rover->turnAvoidDirection));
				}
				else{// There is another closer obstacle, so increase the turn amount
					clearHeading=clearHeading+rk.TURNINCREMENT*rover->turnAvoidDirection;
					turnToHeading(rover, (clearHeading - rover->compass));
				}
			}
			else turnToHeading(rover, (clearHeading - rover->compass));	//continue along the deviated path until past obstacle		
		}
	}
}

// profile laser generates 16 values which cover 90 degreees of angle or 5.625 degrees each
// this function converts those values into height relative to the rover assuming the rover is on flat
//ground.  The function then returns either a -1, 0 or 1 depending on whether it detects a
// significant negative obstacle, flat ground or positive obstacle.
// the distance to the begining of the obstacle is placed in obstDist
// deviation is filled by this function
// The profile laser is only used 1.4m in front of the rover
int terrainProfile(roverProperties *rover, float *obstDist, float *deviation, int *proIdx)
{
	int i, minIdx=1000,maxIdx=1000;
	float minHeight=1000.0, maxHeight=-1000.0;
	proIdx = 0;
	// get all the heights from the profile laser and place them into deviation
	getProfileHeights(rover,deviation);
	
	for(i=0;i<rk.PROFILESEG;i++){
		//printf("%d-%d-%f > %f\n",i,rover->profileScanner->rangeData[i],deviation[i],rk.PROFILEOBSTACLEMAX);
		if(maxIdx > 999 && deviation[i]>rk.PROFILEOBSTACLEMAX){
			maxHeight=deviation[i];
			maxIdx=i;
		}
		if(minIdx > 999 && deviation[i]<rk.PROFILEOBSTACLEMIN){
			minHeight=deviation[i];
			minIdx=i;
		}
	}
	if(minIdx > 999 && maxIdx > 999) return(0);//no significant terrain deviations
	else{
		if(minIdx < maxIdx){
			//printf("profile -- obs\n");
			*obstDist = tan(minIdx*rk.PROFILEINCR)*rk.PROFILEHEIGHT;
			*proIdx=minIdx;
			return(-1);
		}
		else{
			//printf("profile ++ obs\n");
			*obstDist = tan(maxIdx*rk.PROFILEINCR)*rk.PROFILEHEIGHT;
			*proIdx=maxIdx;
			return(1);
		}
		
	}
}

//returns the maximum (or min if negative) slope between rover and rk.Bodydist.
//Does some filtering so changes of more than 15 degrees from previous readings are
//ignored.
float profileNoPosObstacle(roverProperties *rover)
{
	float rise, run, tmpSlope, maxxSlope= -9999.0, minnSlope=9999.0, theSlope;
	int i, lastProfile=0;
	static float profile[2][50], prevSlope= 0.0, prevSlope2=0.0;
	for(i=0;i<rk.PROFILESEG;i++){
		profile[0][i]= rk.PROFILEHEIGHT - cos(i*rk.PROFILEINCR)*(float)rover->profileScanner->rangeData[i]/1000.0;
		profile[1][i]= sin(i*rk.PROFILEINCR)*(float)rover->profileScanner->rangeData[i]/1000.0;
		if(profile[1][i] <= rk.BODYDIST)lastProfile = i;
		//		printf("ri:%.2f ru:%.2f last:%d\n",profile[0][i],profile[1][i], lastProfile);
	}
	if(lastProfile >= (rk.PROFILESEG)) lastProfile = rk.PROFILESEG;
	for(i=0; i+2 <rk.PROFILESEG; i++){
		rise=profile[0][i+2] - profile[0][i];
		run=profile[1][i+2] - profile[1][i];
		if(run==0.0 || run==-0.0)tmpSlope=90.0;
		else tmpSlope=(180.0/PI)*atan2(rise,run);
		if(tmpSlope > maxxSlope) maxxSlope=tmpSlope; 
		//if(maxxSlope  > 90) printf("i:%d PS:%d LP:%d PFrise+2:%.2f PFrun+2:%.2f PFrise:%.2f PFrun:%.2f\n",i,rk.PROFILESEG,lastProfile,profile[0][i+2],profile[1][i+2],profile[0][i],profile[1][i]);
		if(tmpSlope < minnSlope)minnSlope=tmpSlope;
	}
	//if(maxxSlope  > 90) printf("i:%d PS:%d LP:%d PFrise+2:%.2f PFrun+2:%.2f PFrise:%.2f PFrun:%.2f\n",i,rk.PROFILESEG,lastProfile,profile[0][i+2],profile[1][i+2],profile[0][i],profile[1][i]);
	//if(maxxSlope  > 90)for(i=0; i+2 <lastProfile; i++)printf("rise: %.2f   run %.2f  rise: %.2f   run %.2f\n",profile[0][i+2], profile[1][i+2],profile[0][i], profile[1][i]);
	theSlope = (fabs(minnSlope) > maxxSlope) ? minnSlope : maxxSlope;
	if(fabs(theSlope - prevSlope) > 15.0){ // big change from previous slope
		if(fabs(theSlope - prevSlope2) > 15.0){//and previous2 slope, then current slope is probably anomaly
			prevSlope2=prevSlope; prevSlope = theSlope;
			return(prevSlope2);//so return the previous slope
		}
		else{// similar to prev2 slope, so previous slope was probably anomaly
			prevSlope2=prevSlope; prevSlope = theSlope;
			return(theSlope);//current slope
		}
	}
	else{//not a radical change
		prevSlope2=prevSlope; prevSlope = theSlope;
		return(theSlope);//so return the current slope
	}
}



//takes an array of at least PROFILESEG in length and fills the first PRFOFILESEG elements with the heights of the surface
void getProfileHeights(roverProperties *rover, float *heights)
{
	int i;
	for(i=0;i<rk.PROFILESEG;i++) heights[i] = rk.PROFILEHEIGHT - cos(i*rk.PROFILEINCR)*(float)rover->profileScanner->rangeData[i]/1000.0;
}

//takes an array of at least PANELSEG and fills it with the heights from a flat surface.  Note that since the measurements
//are the shortest possible ray paths, the two center segments are calculated as if they are both at an angle 0
//also note that the panel scanner is mounted at 45 degrees, hence the SQRTTWO fact in the height calculation
void getPanelHeights(roverProperties *rover, float heights[])
{
	int i,j;
	
	for(i=0;i<rk.PANELSEGMENTS/2;i++){
		j=i-(rk.PANELSEGMENTS/2 );
		heights[i] = rk.PANELHEIGHT - cos(j*rk.PANELINCR)*((float)rover->panelScanner->rangeData[i])/(SQRTTWO*1000.0);	
		//printf("i:%d j:%d d:%d h:%f.2\n",i,j,rover->panelScanner->rangeData[i], heights[i]);
	}
	for(i=rk.PANELSEGMENTS/2;i<rk.PANELSEGMENTS;i++){
		j=rk.PANELSEGMENTS/2 - i;
		heights[i] = rk.PANELHEIGHT - cos(j*rk.PANELINCR)*((float)rover->panelScanner->rangeData[i])/(SQRTTWO*1000.0);	
		//printf("i:%d j:%d d:%d h:%f.2\n",i,j,rover->panelScanner->rangeData[i], heights[i]);
	}
}
//int main(){}





